
#include "TFAIImpl.h"

TFBoard * currentBoard; //this is here because of a link problem

float Heuristic(Node* current, Node* goal);
float square(float number);

TFAIImpl::TFAIImpl()
{
	for(int i=0; i < BOARD_DIMENSION; i++)
	{
		for(int j=0; j < BOARD_DIMENSION; j++)
		{
			//initial movement cost is 1 
			//since it cost 1 point to move into an empty space
			_boardCost[i][j] = 1; 
		}
	}

	_costMoveList = 999;
	_depthOfMoveList = 999;
	_clearFlagPositions();
	_firstInit=false;
}

TFAIImpl::~TFAIImpl()
{
}

HRESULT TFAIImpl::SetupBoard(TF_PLAYERS iColor)
{
	_myColor = iColor;
	TFBoard * currentBoard = _model->GetBoard();

	//columns to place troops & flags
	int y = 7;

	if(_myColor == BLUE_PLAYER)
	{
		//blue has rows 4-7
		y = 7;
	}else if(_myColor == RED_PLAYER)
	{
		y = 0;
	}
	
	for(int i=0;i<3;i++)
	{
		int x = abs(((int)time(0)*rand()%8));

		if(currentBoard->GetPieceAt(x, y)->GetPiece() == EMPTY &&
			currentBoard->GetPieceAt(x, y+1)->GetPiece() == EMPTY)
		{
			currentBoard->AddPiece(x, y, FLAG, _myColor); 
			currentBoard->AddPiece(x, y + 1, TROOP_TEMP, _myColor);
		}
		else
		{
			i--;
		}
	}

	//Place red barricades.
	for(int i=0;i<6;i++){
		int x = abs(((int)time(0)*rand()%8));
		y = abs(((int)time(0)*rand()%4));

		if(currentBoard->GetPieceAt(x, y)->GetPiece() == EMPTY){
			currentBoard->AddPiece(x, y, BARRICADE, _myColor); 
		}else{
			i--;
		}
	}
	
	return S_OK;
}

HRESULT TFAIImpl::DoTurn(int iPoints, TFGameMoveList & oListOfMoves)
{
	HRESULT HR = S_OK;
	oListOfMoves.Reset();

	//copy current board into the new board
	currentBoard = _model->GetBoard();

	//update internal state from the last moves
	_UpdateBoardCost(currentBoard);

	//Should I act offensively or defensively?
	int behavior = _GetBehavior();

	//Place pieces
	PlacePieces(currentBoard, iPoints, behavior);

	//calculate moves
	if(behavior == OFFENSE)
		HR = _DoOffense2(iPoints, currentBoard, oListOfMoves);
	else if(behavior == DEFENSE)
		HR = _DoDefense(iPoints, currentBoard, oListOfMoves);

	return HR;
}

int TFAIImpl::_GetBehavior()
{
	return OFFENSE;
}


void TFAIImpl::_UpdateBoardCost(TFBoard *& iBoard)
{
	int flagIndex = 0;
	int flagEnemyIndex = 0;
	int troopIndex = 0;
	int troopEnemyIndex = 0;

	_clearFlagPositions();

	for(int i=0; i < BOARD_DIMENSION; i++)
	{
		for(int j=0; j < BOARD_DIMENSION; j++)
		{
			//get piece at current location
			TFGamePiece* tempPiece = iBoard->GetPieceAt(i, j);
			
			//update cost to move through a space
			//ask rule checker for the cost to move through that position
			int cost = _ruleChecker->CostToCapture(tempPiece, _myColor);
			_boardCost[i][j] = cost; 

			//update flag positions
			bool foundFlag = false;
			if(tempPiece->GetPiece() == FLAG_0){foundFlag=true;}
			else if(tempPiece->GetPiece() == FLAG_1){foundFlag=true;}
			else if(tempPiece->GetPiece() == FLAG_2){foundFlag=true;}

			if(foundFlag)
			{
				if(tempPiece->GetColor() == _myColor)
				{
					_boardCost[i][j] = 999;
					_myFlags[troopIndex][0] = i;
					_myFlags[troopIndex][1] = j;
					troopIndex++;
				}else
				{
					_enemyFlags[flagEnemyIndex][0] = i;
					_enemyFlags[flagEnemyIndex][1] = j;
					flagEnemyIndex++;
				}
			}

			//update troop position
			if(tempPiece->GetPiece() == TROOP_PIECE)
			{
				if(tempPiece->GetColor() == _myColor)
				{
					_boardCost[i][j] = 999;
					_myTroops[flagIndex][0] = i;
					_myTroops[flagIndex][1] = j;
					flagIndex++;
				}else
				{
					_enemyTroops[troopEnemyIndex][0] = i;
					_enemyTroops[troopEnemyIndex][1] = j;
					troopEnemyIndex++;
				}			
			}
		}
	}
}

HRESULT TFAIImpl::PlacePieces(TFBoard *& ioBoard, int numPoints, int behavior)
{
	bool canPlaceBarricade = _ruleChecker->CanPlaceBarricade(numPoints);
	bool canPlaceFlagFortification = _ruleChecker->CanPlaceFlagFortification(numPoints);
	bool canPlaceTroop = _model->CanAddTroop(_myColor);

	if(behavior == OFFENSE)
	{
		if(canPlaceTroop)
		{
			_model->TakeTroop(false, _myColor);
			HRESULT HR = E_FAIL;
			while(HR != S_OK)
			{
				int x = abs(((int)time(0)*rand()%8));
				int y = abs(((int)time(0)*rand()%2));
				HR = currentBoard->AddPiece(x, y, TROOP_TEMP, _myColor);
			}

		}

		if(canPlaceBarricade)
		{
			int x =0;
			int y= 0;
			int offset = 0;

			if(_myColor == BLUE_PLAYER){
				offset = 4;
			}else{
				offset = 0;
			}

			//place 1 barricade
			for(int i=0;i<1;i++)
			{
					x = abs(((int)time(0)*rand()%8));
					y = abs((int)time(0)*rand()%4) + offset;

					if(ioBoard->GetPieceAt(x, y)->GetPiece() == EMPTY){
						ioBoard->AddPiece(x, y, BARRICADE, _myColor); 
					}else{
						i--;
					}

			}
		}

		if(canPlaceFlagFortification)
		{
			int flagFullCount = 0;
			int flagCount = 0;
			int flagCountRed = 0;
			int flagCountBlue = 0;
			
			for(int i=0;i<8;i++){
				TF_PIECE_STATE pieceState = ioBoard->GetPieceAt(i, 0)->GetPiece();
				if(pieceState == FLAG_0 || pieceState == FLAG_1 || pieceState == FLAG_2){
					flagCountRed++;
				}
			}

			for(int i=0;i<8;i++){
				TF_PIECE_STATE pieceState = ioBoard->GetPieceAt(i, 7)->GetPiece();
				if(pieceState == FLAG_0 || pieceState == FLAG_1 || pieceState == FLAG_2){
					flagCountBlue++;
				}
			}

			int x =0;
			int y = 0;

			if(_myColor == BLUE_PLAYER){
				y = 7;
				flagCount = flagCountBlue;
			}else{
				y = 0;
				flagCount = flagCountRed;
			}

			for(int i=0;i<1;i++){
				x = abs(((int)time(0)*rand()%8));

				if(ioBoard->GetPieceAt(x, y)->GetPiece() == FLAG_2){
					flagFullCount++;
				}
				
				if((ioBoard->GetPieceAt(x, y)->GetPiece() == FLAG_0) || (ioBoard->GetPieceAt(x, y)->GetPiece() == FLAG_1) ) {
					ioBoard->AddPiece(x, y, FLAG_FORTIFICATION, _myColor); 
					i = 1;
				}else{
					if(flagFullCount != flagCount){
						i--;
					}
				}

			}
		}
	}
	else if(behavior == DEFENSE)
	{
		if(canPlaceBarricade)
		{

		}

		if(canPlaceFlagFortification)
		{

		}
	}

	return S_OK;
}

HRESULT TFAIImpl::_DoOffense(int iPoints, TFBoard *& oBoard, TFGameMoveList & oListOfMoves)
{
	//which troop is closest to a flag
	int nearestFlags[] = {-1,-1,-1}; //will hold index of nearest flag for the corresponding troop index
	int nearestFlagsCost[] = {99,99,99}; //will hold the min cost
	int closestTroop = 0;
	int closestTroopCost = 99;;
	for(int i = 0; i < NUMBER_OF_TROOPS; i++)
	{
		if(_myTroops[i][0] != EMPTYFLAG)
		{
			for(int j = 0; j < NUMBER_OF_FLAGS; j++)
			{
				if(_enemyFlags[j][0] != EMPTYFLAG)
				{
					int xSize = 0;
					int ySize = 0;
					int enemyFlagX = _enemyFlags[j][0];
					int enemyFlagY = _enemyFlags[j][1];
					int myTroopX = _myTroops[i][0];
					int myTroopY = _myTroops[j][1];

					xSize = abs(myTroopX - enemyFlagX);
					ySize = abs(myTroopY - enemyFlagY);

					int minDistance = xSize + ySize;
					if(minDistance < nearestFlagsCost[i])
					{
						nearestFlags[i] = j;
						nearestFlagsCost[i] = minDistance;
					}
				}
			}
		}

		if(nearestFlagsCost[i] < closestTroopCost)
		{
			closestTroop = i;
			closestTroopCost = nearestFlagsCost[i];
		}
	}

	/*
	//PA_OutputText(1,0,9, "Min spaces: %d ", closestTroopCost);
	//PA_OutputText(1,0,11, "ClosestFlag: %d  spaces: %d", nearestFlags[0], nearestFlagsCost[0]);
	//PA_OutputText(1,0,13, "ClosestFlag: %d  spaces: %d", nearestFlags[1], nearestFlagsCost[1]);
	//PA_OutputText(1,0,15, "ClosestFlag: %d  spaces: %d", nearestFlags[2], nearestFlagsCost[2]);
	*/

	//calculate cheapest paths
	int xDirection = 1;
	int yDirection = 1;
	int enemyFlagX = _enemyFlags[nearestFlags[closestTroop]][0];
	int enemyFlagY = _enemyFlags[nearestFlags[closestTroop]][1];
	int myTroopX = _myTroops[closestTroop][0];
	int myTroopY = _myTroops[closestTroop][1];
	if(enemyFlagX < myTroopX) xDirection = -1; //direction to move x 
	if(enemyFlagY < myTroopY) yDirection = -1; //direction to move y 

	TFGameMove initialPosition(myTroopX, myTroopY, 0, 0);
	TFGameMove iTarget(enemyFlagX, enemyFlagY, 0, 0);
	 _costMoveList = 9999;
	 _depthOfMoveList = 9999;
    Node* startState = new Node(new Vertex(myTroopX, myTroopY), 0, 0);
    Node* goalState = new Node(new Vertex(enemyFlagX, enemyFlagY), 0, 0);
	A_Star(startState, &Heuristic, goalState);

	//solution_path
	//when processing move list, if not enough points for a move, stop processing list.
	bool moveSkipped = false;
	int moves = 0;
	for(int i = 0; i < solution_path.size()-1 && !moveSkipped; i++)
	{
		Node* tempNode1 = solution_path[i];
		Node* tempNode2 = solution_path[i+1];
		TFGameMove tempMove(tempNode1->state->x,
							tempNode1->state->y,
							tempNode2->state->x,
			                tempNode2->state->y);

		TFBoard * iBoard = _model->GetBoard();
		TFGamePiece* tempPiece = iBoard->GetPieceAt(tempNode2->state->x, tempNode2->state->y);
		int captureCost = _ruleChecker->CostToCapture(tempPiece, _myColor);

		if(captureCost <= iPoints && moves < 3)
		{
			iPoints = iPoints - captureCost;
			oListOfMoves.Add(tempMove);
			moves++;
		}else
		{
			moveSkipped = true;
		}
	}


	return S_OK;
}

HRESULT TFAIImpl::_DoOffense2(int iPoints, TFBoard *& oBoard, TFGameMoveList & oListOfMoves)
{
	TFBoard * iBoard = _model->GetBoard();

	TFGameMoveList troopMoves1;
	TFGameMoveList troopMoves2;
	TFGameMoveList troopMoves3;

	int cost1 = 0;
	int cost2 = 0;
	int cost3 = 0;

	int lowestCostIndex = 0;
	int secondCostIndex = 1;
	int thirdCostIndex = 2;

	int lowestCostTroop = 999;
	int secondCostTroop = 999;
	int thirdCostTroop = 999;

	int pos=13;

	//loop over each troop
	for(int i = 0; i < 3; i++)
	{
		int myTroopX = _myTroops[i][0];
		int myTroopY = _myTroops[i][1];
		int closestFlag = 0;
		int closestFlagDistance = 999;
		//find closest flag
		for(int j = 0; j < NUMBER_OF_FLAGS; j++)
		{
			if(_enemyFlags[j][0] != EMPTYFLAG)
			{
				int enemyFlagX = _enemyFlags[j][0];
				int enemyFlagY = _enemyFlags[j][1];		

				int xSize = abs(myTroopX - enemyFlagX);
				int ySize = abs(myTroopY - enemyFlagY);

				int minDistance = xSize + ySize;
				if(minDistance < closestFlagDistance)
				{
					closestFlag = j;
					closestFlagDistance = minDistance;
				}
			}
		}

		int enemyFlagX = _enemyFlags[closestFlag][0];
		int enemyFlagY = _enemyFlags[closestFlag][1];

		//search for move list
		Node* startState = new Node(new Vertex(myTroopX, myTroopY), 0, 0);
		Node* goalState = new Node(new Vertex(enemyFlagX, enemyFlagY), 0, 0);
		A_Star(startState, &Heuristic, goalState);

		//build move list
		int moves = 0;
		bool moveSkipped = false;
		int tempPoints = iPoints;
		for(int k = 0; k < solution_path.size()-1; k++)
		{
			Node* tempNode1 = solution_path[k];
			Node* tempNode2 = solution_path[k+1];
			TFGameMove tempMove(tempNode1->state->x,
								tempNode1->state->y,
								tempNode2->state->x,
								tempNode2->state->y);
			TFGamePiece* tempPiece = iBoard->GetPieceAt(tempNode2->state->x, tempNode2->state->y);
			int captureCost = _ruleChecker->CostToCapture(tempPiece, _myColor);
			
			if(captureCost <= tempPoints && moves < 3 && !moveSkipped)
			{
				tempPoints = tempPoints - captureCost;
				if(i == 0)
				{
					troopMoves1.Add(tempMove);
					cost1 += captureCost;
					if(cost1 < lowestCostTroop)
					{
						lowestCostTroop = cost1;
						lowestCostIndex = 1;
					}
					else if(cost1 < secondCostTroop)
					{
						secondCostTroop = cost1;
						secondCostIndex = 1;
					}
					else if(cost1 < thirdCostTroop)
					{
						thirdCostTroop = cost1;
						thirdCostIndex = 1;
					}

				}
				else if(i == 1)
				{
					troopMoves2.Add(tempMove);
					cost2 += captureCost;
					if(cost2 < lowestCostTroop)
					{
						lowestCostTroop = cost2;
						lowestCostIndex = 2;
					}
					else if(cost2 < secondCostTroop)
					{
						secondCostTroop = cost2;
						secondCostIndex = 2;
					}
					else if(cost2 < thirdCostTroop)
					{
						thirdCostTroop = cost2;
						thirdCostIndex = 2;
					}
				}
				else if(i == 2)
				{
					troopMoves3.Add(tempMove);
					cost3 += captureCost;
					if(cost3 < lowestCostTroop)
					{
						lowestCostTroop = cost3;
						lowestCostIndex = 3;
					}
					else if(cost1 < secondCostTroop)
					{
						secondCostTroop = cost3;
						secondCostIndex = 3;
					}
					else if(cost3 < thirdCostTroop)
					{
						thirdCostTroop = cost3;
						thirdCostIndex = 3;
					}
				}
				moves++;
			}else
			{
				moveSkipped = true;
			}
		}
	}//end troop loop
	//PA_OutputText(1,0,17, "listsize1 = %d", troopMoves1.Size() );
	//PA_OutputText(1,0,19, "listsize2 = %d", troopMoves2.Size() );
	//PA_OutputText(1,0,21, "listsize3 = %d", troopMoves3.Size() );

	//which list is the cheapest
	if(lowestCostIndex == 1)
	{
		for(int p = 0; p < troopMoves1.Size(); p++)
		{
			TFGameMove tempMove;
			troopMoves1.Get(p, tempMove);
			oListOfMoves.Add(tempMove);
		}
		iPoints = iPoints - cost1;
	}else if(lowestCostIndex == 2)
	{
		for(int p = 0; p < troopMoves2.Size(); p++)
		{
			TFGameMove tempMove;
			troopMoves2.Get(p, tempMove);
			oListOfMoves.Add(tempMove);
		}
		iPoints = iPoints - cost2;
	}else if(lowestCostIndex == 3)
	{
		for(int p = 0; p < troopMoves3.Size(); p++)
		{
			TFGameMove tempMove;
			troopMoves3.Get(p, tempMove);
			oListOfMoves.Add(tempMove);
		}
		iPoints = iPoints - cost3;
	}

	//make sure we don't have the same first and second cost
	if (lowestCostIndex == secondCostIndex)
	{
		secondCostIndex = thirdCostIndex;
		secondCostTroop = thirdCostTroop;
	}

	//check if there are any points left to spend
	if(iPoints > 0)
	{
		if(secondCostIndex == 1)
		{
			bool skip = false;
			for(int p = 0; p < troopMoves1.Size() && !skip > 0; p++)
			{
				TFGameMove tempMove;
				troopMoves1.Get(p, tempMove);
				TFGamePiece* tempPiece = iBoard->GetPieceAt(tempMove._x1, tempMove._y1);
				int captureCost = _ruleChecker->CostToCapture(tempPiece, _myColor);
				if(iPoints - captureCost > 0)
				{
					oListOfMoves.Add(tempMove);
					iPoints = iPoints - captureCost;
				}else
				{
					skip = true;
				}
			}
			
		}else if(secondCostIndex == 2)
		{
			bool skip = false;
			for(int p = 0; p < troopMoves2.Size() && !skip > 0; p++)
			{
				TFGameMove tempMove;
				troopMoves2.Get(p, tempMove);
				TFGamePiece* tempPiece = iBoard->GetPieceAt(tempMove._x1, tempMove._y1);
				int captureCost = _ruleChecker->CostToCapture(tempPiece, _myColor);
				if(iPoints - captureCost > 0)
				{
					oListOfMoves.Add(tempMove);
					iPoints = iPoints - captureCost;
				}else
				{
					skip = true;
				}
			}
		}else if(secondCostIndex == 3)
		{
			bool skip = false;
			for(int p = 0; p < troopMoves3.Size() && !skip > 0; p++)
			{
				TFGameMove tempMove;
				troopMoves3.Get(p, tempMove);
				TFGamePiece* tempPiece = iBoard->GetPieceAt(tempMove._x1, tempMove._y1);
				int captureCost = _ruleChecker->CostToCapture(tempPiece, _myColor);
				if(iPoints - captureCost > 0)
				{
					oListOfMoves.Add(tempMove);
					iPoints = iPoints - captureCost;
				}else
				{
					skip = true;
				}
			}
		}

	}

	return S_OK;
}



HRESULT TFAIImpl::_DoDefense(int iPoints, TFBoard *& oBoard, TFGameMoveList & oListOfMoves)
{
	return E_FAIL;
}


void TFAIImpl::_clearFlagPositions()
{
	for(int i=0; i < NUMBER_OF_FLAGS; i++)
	{
		_myFlags[i][0] = EMPTYFLAG; 
		_myFlags[i][1] = EMPTYFLAG; 

		_enemyFlags[i][0] = EMPTYFLAG; 
		_enemyFlags[i][1] = EMPTYFLAG; 

		_myTroops[i][0] = EMPTYFLAG; 
		_myTroops[i][1] = EMPTYFLAG; 

		_enemyTroops[i][0] = EMPTYFLAG; 
		_enemyTroops[i][1] = EMPTYFLAG; 
	
	}
}

void TFAIImpl::GetPotentialMoves(TFGameMove &initialPosition, 
								  TFGameMove &iTarget, 
								  TFGameMoveList &oMoveList,
								  bool useRange, bool returnInvalids)
{
	
	//get the range on either side of target
	//2 more and 2 less than x position
	//anything beyond this range, it will be cheaper to destroy
	//objects in it's path
	int minX = iTarget._x1 - 2;
	if(minX < 0 || !useRange) minX = 0;

	int maxX = iTarget._x1 + 2;
	if(maxX > 7 || !useRange) maxX = 7;

	int minY = iTarget._y1 - 2;
	if(minY < 0 || !useRange) minY = 0;

	int maxY = iTarget._y1 + 2;
	if(maxY > 7 || !useRange) maxY = 7;

	//4 possible moves (up, down, left, right)
	//some directions might not be possible (edge of map, your own flag)
	int leftX = initialPosition._x1 - 1;
	int leftY = initialPosition._y1;

	int rightX = initialPosition._x1 + 1;
	int rightY = initialPosition._y1;

	int upX = initialPosition._x1;
	int upY = initialPosition._y1 + 1;

	int downX = initialPosition._x1;
	int downY = initialPosition._y1 - 1;
	
	//add left move if in range and space is not blocked
	if(leftX >= 0 && leftX <= 7 &&
	   leftY >= 0 && leftY <= 7 &&
	   leftX >= minX && leftX <= maxX// ||leftY > minY || leftY < maxY)
	   )
	{
		if(_boardCost[leftX][leftY] != 999){
			TFGameMove tempMove(leftX, leftY, 0, 0);
			if(!_myMoveList.IsInList(tempMove))
			{
				oMoveList.Add(tempMove);
			}
		}
	}
	//add right move if in range and space is not blocked
	if(rightX >= 0 && rightX <= 7 &&
	   rightY >= 0 && rightY <= 7 &&
	   rightX >= minX && rightX <= maxX// ||rightY > minY || rightY < maxY)
	   )
	{
		if(_boardCost[rightX][rightY] != 999){
			TFGameMove tempMove(rightX, rightY, 0, 0);
			if(!_myMoveList.IsInList(tempMove))
			{
				oMoveList.Add(tempMove);
			}
		}
	}

	//add up move if in range and space is not blocked
	if(upX >= 0 && upX <= 7 &&
	   upY >= 0 && upY <= 7 &&
	   upX >= minX && upX <= maxX// || upY > minY || upY < maxY)
	   )
	{
		if(_boardCost[upX][upY] != 999)
		{
			TFGameMove tempMove(upX, upY, 0, 0);
			if(!_myMoveList.IsInList(tempMove))
			{
				oMoveList.Add(tempMove);
			}
		}
	}

	//add down move if in range and space is not blocked
	if(downX >= 0 && downX <= 7 &&
	   downY >= 0 && downY <= 7 &&
	   downX >= minX && downX <= maxX// || downY > minY || downY < maxY)
	   )
	{
		if(_boardCost[downX][downY] != 999)
		{
			TFGameMove tempMove(downX, downY, 0, 0);
			if(!_myMoveList.IsInList(tempMove))
			{
				oMoveList.Add(tempMove);
			}
		}
	}
}


/** Returns the straight line distance from current state to goal state. */
float Heuristic(Node* current, Node* goal){
	float distance2 = sqrt(square(goal->state->x - current->state->x) + square(goal->state->y - current->state->y));
	
	float xSize = abs((int)goal->state->x - (int)current->state->x);
	float ySize = abs((int)goal->state->y - (int)current->state->y);
	float distance = xSize + ySize;

	return distance + distance2;
}

/** Returns a vector of all possible next states (visable verticies) */
vector <Node*> TFAIImpl::Successor(Node* current){
	vector <Node*> nextStates;

	TFGameMove initialPosition((int)current->state->x, (int)current->state->y, 0, 0); 
	TFGameMove iTarget(0, 0, 0, 0);
	TFGameMoveList oMoveList;

	GetPotentialMoves(initialPosition,
					 iTarget,
					 oMoveList,
					 false,
					 false);

	for(int j = 0; j < oMoveList.Size(); j++)
	{
		TFGameMove oGameMove;
		oMoveList.Get(j, oGameMove);
		TFBoard * iBoard = _model->GetBoard();
		TFGamePiece* tempPiece = iBoard->GetPieceAt(oGameMove._x1, oGameMove._y1);
		int cost = _ruleChecker->CostToCapture(tempPiece, _myColor);
		Node* tempNode = new Node(new Vertex((float)oGameMove._x1, (float)oGameMove._y1), current, cost+1); 
	
		nextStates.push_back(tempNode);
	}

	return nextStates;
}



/** Classic A* algorithm with repeat queue & ancestor state checking */
void TFAIImpl::A_Star(Node* initState, hFunction myHeuristic, Node* goalState){
	
	solution_path.clear();
	//Quick goal test to verify different state inputs;
	if(goalTest(initState, goalState)){
		//cout << "\nInitial state can not be the goal state...";
	}

	int totalNodesGenerated = 1;	//Tracks number of nodes generated;
//	bool isAncestor = false;		//Tracks if node's state exists in ancestor;
	int totalExpanded = 0;			//Tracks total nodes expanded;
	vector<Node*> newQItems;;		//Holds newly generated successor states;
	Node* root = new Node();		//Root of state tree;
	Node* currentNode;				//Holds current node taken from queue;
	Node* tempNode;					//Used to iterate through parent nodes;
	root->state = initState->state;
	root->setfValue(myHeuristic(initState,goalState));
	bool solution_found = false;
	Node* solution_node = 0;
	float distance = 0;

	Queue* myQ = new Queue();
	myQ->add(root);

	while(myQ->getSize() > 0){
		//Removes node with lowest f(n) from queue;
		currentNode = myQ->remove();
		
		
		if((solution_found == false) || (currentNode->getfValue() < solution_node->getfValue())  ){
			//Always goalTest() before expanding current node;
			if(goalTest(currentNode, goalState)){
				solution_found = true;
				solution_node = currentNode;
				distance = currentNode->getPathCost();
				
			}else{
				//Get's successor nodes;
				newQItems = Successor(currentNode);		
				for(int i=0;i<(int)newQItems.size();i++){
					tempNode = newQItems.at(i);
					tempNode->setfValue((myHeuristic(tempNode,goalState))+ tempNode->getPathCost());
					myQ->add(newQItems.at(i));
					totalNodesGenerated++;
				}
			}
			totalExpanded++;
			//If it's a long search give screen updates of status;
			if(totalExpanded%10 == 0){
				//PA_OutputText(1,0,17, "totalExpanded %d", totalExpanded);
				//PA_OutputText(1,0,21, "path cost %d", currentNode->getPathCost());
	
			}
		}
	}

	printSolution(solution_node);

	return;
}

/** Returns true if two nodes have the same state. */
bool TFAIImpl::goalTest(Node* current, Node* goal){
	if(current->state->x == goal->state->x && current->state->y == goal->state->y){
		return true;
	}else{
		return false;
	}
}

/** Travels back up a path and prints entire solution. */
void TFAIImpl::printSolution(Node* myNode){
	
	if(myNode->hasParent()){
		printSolution(myNode->getParent());
	}

	solution_path.push_back(myNode);
}

float square(float number){
	return number * number;
}

